#include "camera_projection.h"
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

CameraProjection::CameraProjection(const Eigen::Matrix4d& extrinsic_matrix,std::string& camera_name,bool view_image=false)
{
    extrinsic_matrix_ = extrinsic_matrix;
    view_image_ = view_image;
    camera_info_init_ = false;
    window_name_ = camera_name;
}

void CameraProjection::set_detect_client(ros::ServiceClient& client)
{
    detect_client = client;
}

void CameraProjection::distance_normal(std::vector<double>& distance)
{
    double min = DBL_MAX;
    double max = -DBL_MAX;
    for(auto d:distance)
    {
        min = std::min(min,d);
        max = std::max(max,d);
    }

    for(int i=0;i<distance.size();i++)
    {
        distance[i] = (distance[i]-min)/(max-min);
    }
}

void CameraProjection::image_callback(const sensor_msgs::ImageConstPtr& msg)
{
    auto start = ros::Time::now();
    migratable_rl::detect detect_srv;
    detect_srv.request.Image = *msg;
    if(!detect_client.call(detect_srv))
    {
        ROS_INFO("Detect node isn't running");
        return ;
    }
    detect_result_ = detect_srv.response.bounding_box;
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    if(image_position_.size() != 0)
    {
        visualize_point(cv_ptr->image,detect_srv.response);
    }
    if(view_image_)
    {
        show_bounding_box(cv_ptr->image,detect_srv.response);
        cv::imshow(window_name_, cv_ptr->image);
    }
    std::cout<<"use_time:"<<ros::Time::now()-start<<std::endl;
}

void CameraProjection::show_bounding_box(cv::Mat&image,migratable_rl::detectResponse& detect_result)
{
    for(int i=0;i<detect_result.bounding_box.type.size();i++)
    {
        int x_min = detect_result.bounding_box.bounding_box[i].x_min;
        int x_max = detect_result.bounding_box.bounding_box[i].x_max;
        int y_min =  detect_result.bounding_box.bounding_box[i].y_min;
        int y_max =  detect_result.bounding_box.bounding_box[i].y_max;
        cv::rectangle(image,cv::Point(x_min,y_min),cv::Point(x_max,y_max),cv::Scalar(255,0,0),3);
    }
}

int CameraProjection::in_boundingbox(cv::Point2i& p,const migratable_rl::boundingbox& bounding_box)
{
    for(int i=0;i<bounding_box.type.size();i++)
    {
        int x_min = bounding_box.bounding_box[i].x_min;
        int x_max = bounding_box.bounding_box[i].x_max;
        int y_min =  bounding_box.bounding_box[i].y_min;
        int y_max =  bounding_box.bounding_box[i].y_max;
        if(p.x>x_min && p.x<x_max && p.y>y_min && p.y<y_max)
            return bounding_box.type[i];
        else
            continue;
    }
    return -1;
}

void CameraProjection::visualize_point(cv::Mat& image,const migratable_rl::detectResponse& detect_result)
{
    std::vector<cv::Point2i> position;
    std::vector<double> distance;

    for(auto& lidar_point:image_position_)
    {
        cv::Point2i p(lidar_point.x,lidar_point.y);
        distance.emplace_back(lidar_point.z/100.0);
        position.emplace_back(p);
    }   

    distance_normal(distance);
    
    for(int i=0;i<position.size();i++)
    {
        if(type_[i] == -1)
        {
            cv::circle(image, position[i], 3, cv::Scalar(255,255,255), 3);
        }
        else
        {
            if(type_[i] == 0)
                cv::circle(image, position[i], 3, cv::Scalar(255,0,0), 3);
            else if(type_[i] == 1)
                cv::circle(image, position[i], 3, cv::Scalar(0,255,0), 3);
            else
                cv::circle(image, position[i], 3, cv::Scalar(0,0,255), 3);
        }
    }
}



void CameraProjection::camerainfo_callback(const sensor_msgs::CameraInfoConstPtr& msg)
{
    if(!camera_info_init_)
    {
        camera_info_init_ = true;
        for(int i=0;i<msg->K.size();i++)
        {
            camera_matrix_(i/3,i%3) = msg->K[i];
        }
        image_height_ = msg->height;
        image_width_ = msg->width;
        std::cout<<camera_matrix_<<std::endl;
    }
}

void CameraProjection::reset_vector()
{
    image_position_.clear();
    type_.clear();
}

void CameraProjection::points_projection(std::vector<Eigen::Vector3d>& points)
{
    reset_vector();
    for(auto& p:points)
    {
        point_projection(p);
    }
}

int CameraProjection::point_projection(Eigen::Vector3d& point)
{
    Eigen::Vector4d point_in_base(point(0),point(1),point(2),1);

    Eigen::Vector4d point_in_camera = extrinsic_matrix_* point_in_base;

    double x = point_in_camera(0);
    double y = point_in_camera(1);
    double z = point_in_camera(2);


    if(x<0)   //排除三四象限的点
        return -1;
    double col = -y/x*camera_matrix_(0,0)+camera_matrix_(0,2);
    double row = z/x*camera_matrix_(1,1)+camera_matrix_(1,2);
    cv::Point3i p;
    p.x = round(col);
    p.y = round(row);
    p.z = int(sqrt(x*x+y*y)*100);

    if(p.x>=image_width_ || p.x<0 || p.y>=image_height_ || p.y<0)
        return -1;

    cv::Point2i p2i(p.x,p.y);
    int idx = in_boundingbox(p2i,detect_result_);
    image_position_.emplace_back(p);
    type_.emplace_back(idx);
    return idx;
}